#include "costmap_generater_core.h"

CostmapGenerater::CostmapGenerater():nh_(""), private_nh_("~"), tf2_listener(buffer){
    private_nh_.param<std::string>("output_costmap_frame_id", output_costmap_frame_id, "");
    private_nh_.param<std::string>("output_costmap_topic", output_costmap_topic, "");
    private_nh_.param<std::string>("input_confusion_pc2_topic", input_confusion_pc2_topic, "");
   
    private_nh_.param<double>("costmap_resolution", costmap_resolution, 0.1);
    private_nh_.param<int>("costmap_height", costmap_height, 0);
    private_nh_.param<int>("costmap_width", costmap_width, 0);
}

CostmapGenerater::~CostmapGenerater(){}

bool CostmapGenerater::valid_point_in_map(double x, double y){
    if(x<initial_costmap_.info.origin.position.x||y<initial_costmap_.info.origin.position.y){
        return false;
    }
    int mx, my;
    mx = (int)((x-initial_costmap_.info.origin.position.x)/initial_costmap_.info.resolution);
    my = (int)((y-initial_costmap_.info.origin.position.y)/initial_costmap_.info.resolution);
    if(mx<initial_costmap_.info.width&&my<initial_costmap_.info.height){
        return true;
    }
    return false;
}

int CostmapGenerater::get_index(double x, double y){
    int mx, my;
    mx = (int)((x-initial_costmap_.info.origin.position.x)/initial_costmap_.info.resolution);
    my = (int)((y-initial_costmap_.info.origin.position.y)/initial_costmap_.info.resolution);
    return my*initial_costmap_.info.width+mx;
}

void CostmapGenerater::confusion_pointcloud2_call_back(const::sensor_msgs::PointCloud2::ConstPtr& msg){
    confusion_pointcloud2_ = *msg;
}


void CostmapGenerater::init_cost_map(){
    initial_costmap_.header.frame_id = output_costmap_frame_id;
    initial_costmap_.info.height = costmap_height;
    initial_costmap_.info.width = costmap_width;
    initial_costmap_.info.resolution = costmap_resolution;
    initial_costmap_.info.origin.position.x = -1.5;
    initial_costmap_.info.origin.position.y = -1.0*costmap_height*costmap_resolution/2.0;
    initial_costmap_.info.origin.position.z = 0.0;
    initial_costmap_.info.origin.orientation.x = 0.0;
    initial_costmap_.info.origin.orientation.y = 0.0;
    initial_costmap_.info.origin.orientation.z = 0.0;
    initial_costmap_.info.origin.orientation.w = 1.0;
    for(unsigned int i=0; i<costmap_height*costmap_width; i++){
        initial_costmap_.data.push_back(0);
    }
}

void CostmapGenerater::process(const ros::TimerEvent& e){
    nav_msgs::OccupancyGrid cost_map_ = initial_costmap_;
    cost_map_.header.stamp = ros::Time().now();
    sensor_msgs::PointCloud2 pointcloud2_trans;
    geometry_msgs::TransformStamped transform_stamed;
    geometry_msgs::Transform transform;
    Eigen::Matrix4f eigen_transform;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_pointcloud2_trans(new pcl::PointCloud<pcl::PointXYZ>());
    int obstacle_count=0;
    int obstacle_count_gezi =0;
    
    

    /* 融合点云往栅格投影 */
    if(fabs(ros::Time().now().toSec()-confusion_pointcloud2_.header.stamp.toSec())<0.5){
        try{
            transform_stamed = buffer.lookupTransform(output_costmap_frame_id, confusion_pointcloud2_.header.frame_id, ros::Time().now(), ros::Duration(1.0));
            transform.translation = transform_stamed.transform.translation;
            transform.rotation = transform_stamed.transform.rotation;
            pcl_ros::transformAsMatrix(transform, eigen_transform);
            pcl_ros::transformPointCloud(eigen_transform, confusion_pointcloud2_, pointcloud2_trans);
            pointcloud2_trans.header.frame_id = output_costmap_frame_id;
            pcl::fromROSMsg(pointcloud2_trans, *pcl_pointcloud2_trans);
            for(const auto& p:pcl_pointcloud2_trans->points){
                if(valid_point_in_map(p.x, p.y)){
                    cost_map_.data[get_index(p.x, p.y)] = 100;
                    obstacle_count++;
                }
            }
            obstacle_maxnum = std::max(obstacle_maxnum, obstacle_count);
            // std::cout<<"obstacle_count is "<<obstacle_count;
            // std::cout<<" obstacle_maxnum is :"<<obstacle_maxnum<<std::endl;
        }catch (tf2::TransformException &ex){
            ROS_WARN("[%s] transform %s to %s error......", __APP_NAME__, confusion_pointcloud2_.header.frame_id.c_str(), output_costmap_frame_id.c_str());
        }
    }

    for(int i=0; i<cost_map_.data.size(); i++){
        if(cost_map_.data[i]==100){
            obstacle_count_gezi++;
        }
    }
    // ROS_INFO("[%s] obstacle_count_gezi is %d", __APP_NAME__, obstacle_count_gezi);
    cost_map_pub.publish(cost_map_);
}

void CostmapGenerater::run(){
    init_cost_map();
    cost_map_pub = nh_.advertise<nav_msgs::OccupancyGrid>(output_costmap_topic, 1);
    confusion_pointcloud2_sub = nh_.subscribe<sensor_msgs::PointCloud2>(input_confusion_pc2_topic, 1, &CostmapGenerater::confusion_pointcloud2_call_back, this);
    ros::Timer timer = nh_.createTimer(ros::Duration(0.2), &CostmapGenerater::process, this, false, true);
    ros::spin();
}